
#include "nimservosdk.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#ifdef _WIN32
#include <Windows.h>
#define MySleepMS(msecs) Sleep(msecs)
#else
#include <unistd.h>
#include <string.h>
#define MySleepMS(msecs) usleep(msecs*1000)
#endif

int main(int argc, char* argv[])
{
    if (argc < 3)
    {
        printf("Usage: NimServoSDK_Test2 commType commParam [unitFactor]\r\n");
        return 0;
    }

    if (0 != Nim_init(atoi(argv[1])))
    {
        printf("exec Nim_init faild\n");
        //return -1;
    }
    if (atoi(argv[1]) < 0 || atoi(argv[1]) > 2)
        return 0;

    char conn_str[200] = {0};
    switch (atoi(argv[1])) {
    case 0:     //CANopen
        switch (atoi(argv[2])) {
        case 1001:      //NiMotion USB-CAN Device
            sprintf(conn_str, "{\"DevType\": \"%s\", \"DevIndex\": 0, \"Baudrate\": 8,"\
                              " \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 10}", argv[2]);
            break;
        case 1002:      //Zhoulg USB-CAN Device
            sprintf(conn_str, "{\"DevType\": \"%s\", \"ChannelNum\": 1, \"DevIndex\": 0, \"ChannelIndex\": 0,"\
                              " \"Baudrate\": 8, \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 10}", argv[2]);
            break;
        case 1003:      //Ixxat USB-CAN Device
            sprintf(conn_str, "{\"DevType\": \"%s\", \"DevIndex\": 0, \"Baudrate\": 8,"\
                              " \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 10}", argv[2]);
            break;
        case 1004:      //NiMotion TCP-CAN Device
            sprintf(conn_str, "{\"DevType\": \"%s\", \"IP\": \"192.168.0.96\", \"Port\": 40001,"\
                              " \"Baudrate\": 8, \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 10}", argv[2]);
            break;
        case 1005:      //Linux SocketCAN Device
            sprintf(conn_str, "{\"DevType\": \"%s\", \"DeviceName\": \"can0\","\
                              " \"Baudrate\": 8, \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 10}", argv[2]);
            break;
        default:
            sprintf(conn_str, "{\"DevType\": \"%s\", \"DevIndex\": 0, \"Baudrate\": 8,"\
                              " \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 10}", "1001");
            break;
        }
        break;
    case 1:     //EtherCAT
        sprintf(conn_str, "{\"NetworkAdapter\": \"%s\", \"OverlappingPDO\": true, \"PDOIntervalMS\": 5}", argv[2]);
        break;
    case 2:     //Modbus
        sprintf(conn_str, "{\"SerialPort\": \"%s\", \"Baudrate\": 115200, \"Parity\": \"N\", \"DataBits\": 8, \"StopBits\": 1,"\
                          " \"PDOIntervalMS\": 10, \"SyncIntervalMS\": 0}", argv[2]);
        break;
    default:
        break;
    }

    printf("%s\r\n", conn_str);

    if (0 != Nim_master_run(conn_str))
    {
        printf("exec Nim_master_run faild\n");
        return -1;
    }

    int nAddr = 0;
    int i = 0;
    Nim_scan_nodes(1, 10);
    for (i=0; i<10; i++)
    {
        if (0 != Nim_is_online(i))
        {
            nAddr = i;
            printf("motor %d is online\n", i);
            break;
        }
    }
    if (nAddr == 0)
    {
        printf("There is no motor online\n");
        Nim_master_stop();
        return -1;
    }

    double fUnitFactor = 10000.0;
    if (argc > 3)
    {
        fUnitFactor = atof(argv[3]);
    }
    printf("UnitFactor = %f\r\n", fUnitFactor);

    int nRe = 0;
    if (atoi(argv[1]) == 2)
        nRe = Nim_load_params(nAddr, "BLMxx_modbus_zh.db");
    else
        nRe = Nim_load_params(nAddr, "Servo_zh.db");
    if (0 != nRe)
    {
        printf("exec Nim_load_params failed\r\n");
        return -1;
    }

    nRe = Nim_read_PDOConfig(nAddr);
    if (0 != nRe)
    {
        printf("exec Nim_read_PDOConfig failed\r\n");
        return -1;
    }

    nRe = Nim_master_changeToOP();
    nRe = Nim_set_unitsFactor(nAddr, fUnitFactor);

    MySleepMS(200);
    nRe = Nim_power_off(nAddr, 1);
    MySleepMS(200);
    nRe = Nim_set_workMode(nAddr, 1, 1);
    MySleepMS(200);
    nRe = Nim_power_on(nAddr, 1);
    MySleepMS(200);
    nRe = Nim_set_profileVelocity(nAddr, 5.0);
    nRe = Nim_set_profileAccel(nAddr, 2.5);
    nRe = Nim_set_profileDecel(nAddr, 2.5);
    printf("********************move relative*********************\n");
    nRe = Nim_moveRelative(nAddr, 20.0, 0, 1);
    double fPos = 0.0;
    unsigned short statusWord = 0;
    do
    {
        MySleepMS(50);
        if (0 == Nim_get_currentPosition(nAddr, &fPos, 1)
            && 0 == Nim_get_statusWord(nAddr, &statusWord, 1))
            printf("statusword = %04x \t currentpos = %f\t\r", statusWord, fPos);
    }while ((statusWord & 0x400) == 0);
    printf("\n");

    MySleepMS(200);
    printf("********************move absolute*********************\n");
    nRe = Nim_moveAbsolute(nAddr, 0.0, 0, 1);
    do
    {
        MySleepMS(50);
        if (0 == Nim_get_currentPosition(nAddr, &fPos, 1)
                && 0 == Nim_get_statusWord(nAddr, &statusWord, 1))
            printf("statusword = %04x \t currentpos = %f\t\r", statusWord, fPos);
    }while ((statusWord & 0x400) == 0);
    printf("\n");

    nRe = Nim_power_off(nAddr, 1);
    MySleepMS(100);
    nRe = Nim_master_changeToPreOP();
    nRe = Nim_master_stop();
    Nim_clean();

    return 0;
}
